#include <cmath>

#include <rclcpp/rclcpp.hpp>
#include <gazebo_msgs/msg/model_states.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/utils.h>

#include "planner_node.h"
#include "path_follower_node.h"
#include <yaml-cpp/yaml.h>
const float WHEELBASE      = 0.877;   // 轴距
const float WHEEL_RADIUS   = 0.115;   // 轮子半径
const float STEER_VELOCITY = M_PI_2;  // 舵机转向速度
// 是否使用角度控制
const bool cmd_angle_instead_rotvel = true;

class SimControlNode : public rclcpp::Node {
public:
    SimControlNode() : Node("sim_control_node"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {
        // 创建TF广播器
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

        // 订阅/cmd_vel话题，解析控制指令，发送到ros2控制器，以驱动小车运动
        subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10, std::bind(&SimControlNode::CmdVelCallback, this, std::placeholders::_1));

        // 订阅/model_states话题, 用于发布机器人位姿
        model_subscription_ = this->create_subscription<gazebo_msgs::msg::ModelStates>(
            "/model_states", 10, std::bind(&SimControlNode::modelStatesCallback, this, std::placeholders::_1));

        // 发布机器人位姿
        odometry_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);

        // 创建发布者，发布到/forward_velocity_controller/commands话题，用于驱动舵轮前进
        velocity_publisher_ =
            this->create_publisher<std_msgs::msg::Float64MultiArray>("/forward_velocity_controller/commands", 10);

        // 创建发布者，发布到/joint_trajectory_controller话题，用于驱动舵轮旋转
        trajectory_publisher_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>(
            "joint_trajectory_controller/joint_trajectory", 10);
    }

private:
    // 获取slim车体的位姿
    void modelStatesCallback(const gazebo_msgs::msg::ModelStates::SharedPtr msg) {
        // 获取slim车体模型
        std::string target_model_name = "slim";
        auto        it                = std::find(msg->name.begin(), msg->name.end(), target_model_name);
        if (it == msg->name.end()) {
            RCLCPP_WARN(this->get_logger(), "Model not found in /model_states");
            return;
        }
        size_t model_index = std::distance(msg->name.begin(), it);

        nav_msgs::msg::Odometry odom;
        odom.header.stamp    = this->get_clock()->now();
        odom.header.frame_id = "map";
        odom.child_frame_id  = "base_link";
        odom.pose.pose       = msg->pose[model_index];
        odom.twist.twist     = msg->twist[model_index];
        odometry_publisher_->publish(odom);

        // 创建 TF 变换消息
        geometry_msgs::msg::TransformStamped transform_stamped;
        transform_stamped.header.stamp            = this->get_clock()->now();
        transform_stamped.header.frame_id         = "odom";
        transform_stamped.child_frame_id          = "base_footprint";
        transform_stamped.transform.translation.x = odom.pose.pose.position.x;
        transform_stamped.transform.translation.y = odom.pose.pose.position.y;
        transform_stamped.transform.translation.z = odom.pose.pose.position.z;
        transform_stamped.transform.rotation      = odom.pose.pose.orientation;
        tf_broadcaster_->sendTransform(transform_stamped);
    }

    // 将接收的线速度和角速度，发送到ros2控制器，以驱动小车运动
    void CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
        // 获取线速度和角速度
        float linear_velocity  = msg->linear.x + 1e-6;
        float angular_velocity = msg->angular.z;

        // 限制舵机速度
        LimitVelocity(linear_velocity, angular_velocity);

        // 计算角速度对应的转向角度
        // TODO:单舵机模型运动学解算:https://www.docin.com/p-769836888.html, 后面会有其他的模型
        // ω = (v * sinθ) / b， 其中b是轮距
        // θ = arcsin((ω * b) / v)
        float radian = std::asin((angular_velocity * WHEELBASE) / linear_velocity);

        // cmd_angle_instead_rotvel为true时，angular_velocity是舵机角度
        // 同时输出原始线速度
        if (cmd_angle_instead_rotvel) {
            linear_velocity = msg->linear.x;
            radian          = msg->angular.z;
        }

        // 发布线速度到forward_velocity_controller控制器
        auto velocity_msg = std_msgs::msg::Float64MultiArray();
        velocity_msg.data.push_back(linear_velocity / WHEEL_RADIUS);
        velocity_publisher_->publish(velocity_msg);

        // 发布舵机角度到joint_trajectory_controller控制器
        auto trajectory_msg        = trajectory_msgs::msg::JointTrajectory();
        trajectory_msg.joint_names = {"steer_wheel_joint"};

        auto point      = trajectory_msgs::msg::JointTrajectoryPoint();
        point.positions = {radian};

        // 根据当前舵机角和期望舵机角的偏差计算时间，用于设置舵机旋转时间
        // TODO:舵机实际响应速度和设置不符，控制频率太高，或者dt太小，舵机不一定会旋转
        geometry_msgs::msg::TransformStamped transform_stamped;
        try {
            // 获取TF变换
            transform_stamped = tf_buffer_.lookupTransform("base_link", "steer_wheel", tf2::TimePointZero);
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s", "base_link", "map", ex.what());
        }

        float cur_radian      = tf2::getYaw(transform_stamped.transform.rotation);
        float dt              = std::abs(cur_radian - radian) / STEER_VELOCITY;
        point.time_from_start = rclcpp::Duration::from_seconds(dt);
        trajectory_msg.points.push_back(point);
        trajectory_publisher_->publish(trajectory_msg);

        // 输出线速度和舵机角度
        // RCLCPP_INFO(this->get_logger(), "real Linear Velocity: %.2f m/s, steer_angle: %.2f rad,ratation_radius:%.2f",
        //             linear_velocity, radian, linear_velocity / angular_velocity);
    }

    void LimitVelocity(float &linear_velocity, float &angular_velocity) {

        // 计算转弯半径
        float steer_radius = std::abs(linear_velocity / angular_velocity);

        // 当转向半径小于轴距时，进行特殊处理
        if (steer_radius < WHEELBASE) {
            angular_velocity = std::copysign(0.5, angular_velocity);
            linear_velocity  = std::abs(angular_velocity * WHEELBASE);
        }
    }

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr          subscription_;
    rclcpp::Subscription<gazebo_msgs::msg::ModelStates>::SharedPtr      model_subscription_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr      velocity_publisher_;
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr               odometry_publisher_;

    // tf变换
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    tf2_ros::Buffer                                tf_buffer_;
    tf2_ros::TransformListener                     tf_listener_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);

    auto simulator     = std::make_shared<SimControlNode>();
    auto planner       = std::make_shared<GlobalPlanner>();
    auto path_follower = std::make_shared<PathFollower>();

    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(simulator);
    executor.add_node(planner);
    executor.add_node(path_follower);

    executor.spin();

    rclcpp::shutdown();

    return 0;
}